#include <SPI.h>
#include <LoRa.h>
#include "config.h"
#include "FlySkyiBusCombined.h"

#define IBUS_MAXCHANNELS  14    // iBus has a maximum of 14 channels
#define IBUS_LOWER_LIMIT  1000
#define IBUS_UPPER_LIMIT  2000

#define GPIO_BIND 8
#define GPIO_LED_INDICATOR 3

HardwareSerial &iBus_serial = Serial;
static uint8_t channel_count = IBUS_MAXCHANNELS;
static uint16_t channel_data [IBUS_MAXCHANNELS];// = {1500, 1500, 1000, 1500, 1020, 1200, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000};
//static uint8_t lazy_packets_count = 20;
void set_safe_outputs (uint16_t *channel_data_in); // To set all outputs to safe values
static int32_t rssiDBM = 0;

void setup() {
  //  delay(2);
  //  iBus.begin(iBus_serial); // Open the iBus serial connection
  //  delay(2);
  //  Serial.begin(115200); // Serial Monitor
  //  iBus.begin(iBus_serial);
  Serial.begin(115200);
  while (!Serial);

  Serial.println("LoRa Receiver");

  pinMode(GPIO_BIND, INPUT_PULLUP);
  pinMode(GPIO_LED_INDICATOR, OUTPUT);
  digitalWrite(GPIO_LED_INDICATOR, LOW);

  LoRa.setSPIFrequency(16E6);
  if (!LoRa.begin(915E6)) {
    Serial.println("Starting LoRa failed!");
    while (1);
  }

  int bindMode = digitalRead(GPIO_BIND);
  if (bindMode == HIGH)
  {
    /* bind code */
  }
  digitalWrite(GPIO_LED_INDICATOR, HIGH);

  LoRa.enableCrc();
  LoRa.setSignalBandwidth(500E3);
}

void loop() {
  // try to parse packet
  int packetSize = LoRa.parsePacket();
  int charI = 0;
  char received[packetSize] = "";
  String received_to_string = "";
  if (packetSize) {
    // received a packet
    //    Serial.print("Received packet: ");
    //    Serial.println(packetSize);

    // read packet
    for (int i = 0; i < packetSize; i++) {
      received[i] = (char)LoRa.read();
    }
    received_to_string = String(received);
    //            Serial.print(received_to_string);
    //            Serial.print(" ");
    //            Serial.print(strtol(received_to_string.substring(0, 3).c_str(), NULL, 16));
    //            Serial.print(strtol(received_to_string.substring(3, 6).c_str(), NULL, 16));
    //            Serial.print(strtol(received_to_string.substring(6, 9).c_str(), NULL, 16));
    //            Serial.print(strtol(received_to_string.substring(9, 12).c_str(), NULL, 16));
    //            Serial.println("");

    // print RSSI of packet
    //        Serial.print("' with RSSI ");
    //        Serial.print(LoRa.packetRssi());
    //        Serial.print("' with rssi ");
    //        Serial.print(LoRa.rssi());
    //        Serial.print(" with SNR ");
    //        Serial.print(LoRa.packetSnr());
    //        Serial.print(" with ERR ");
    //        Serial.println(LoRa.packetFrequencyError());
    if (packetSize == 12 || packetSize == 42) {
      for (int i = 0; i < packetSize / 3; i++) {
        channel_data[i] = strtol(received_to_string.substring(i * 3, i * 3 + 3).c_str(), NULL, 16);
      }
      rssiDBM = LoRa.packetRssi();
      channel_data[13] = map(constrain(rssiDBM, RXsensitivity, -50), RXsensitivity, -50, 1000, 2000);
      Serial.println(channel_data[13]);
      iBus.write_one_frame(channel_data, channel_count, iBus_serial); // Send one frame to be written
    }
  }

}
